#include "Obstacle.h"
#include <iostream>
#include "tinyxml.h"
#include "gl/glut.h"

Obstacle::Obstacle(void)
	: isBoundingBox_(false), isClosed_(false)
{
}


Obstacle::~Obstacle(void)
{
}

bool Obstacle::parseXml(TiXmlElement* node)
{
	vertices_.clear();

	int iVal;
	// First parse the attributes: bounding box and closed
	if ( node->Attribute( "boundingbox", &iVal ) ) {
		isBoundingBox_ = (iVal != 0);
	}
	if ( node->Attribute( "closed", &iVal ) ) {
		isClosed_ = (iVal != 0);
	}

	// Now parse vertices
	double dVal;
	bool valid = true;
	
	for ( TiXmlElement * vert = node->FirstChildElement(); vert; vert = vert->NextSiblingElement() ) {
		if ( vert->ValueStr() == "Vertex") {
			float p_x = 0;
			float p_y = 0;
			if ( vert->Attribute( "p_x", &dVal) ) {
				p_x = (float)dVal;
			} else {
				valid = false;
			}
			if ( vert->Attribute( "p_y", &dVal) ) {
				p_y = (float)dVal;
			} else {
				valid = false;
			}

			if ( ! valid ) {
				std::cerr << "Missing full x-/y- values in Obstacle Vertex!" << std::endl;
				return false;
			}
			vertices_.push_back( Vector2( p_x, p_y ) );
		} else {
			valid = false;
		}

		if ( ! valid ) {
			std::cerr << "Missing attributes in Obstacle!" << std::endl;
			return false;
		}
	}

	// Here is where you process the obstacle data -- in this case, I'm just summarizing it.
	std::cout << "Parsed ";
	if ( isClosed_ ) {
		std::cout << "a closed ";
	} else {
		std::cout << "an open ";
	}
	std::cout << "obstacle with " << vertices_.size() << " vertices";
	if ( isBoundingBox_ ) {
		std::cout << " which serves as a bounding box";
	}
	std::cout << "\n";
	return true;
}

void Obstacle::render()
{
	if (isClosed_)
		glBegin(GL_LINE_LOOP);                      
	else
		glBegin(GL_LINE_STRIP);                      
	glColor3f(1.0f, 0.0f, 0.0f);  	
	for (unsigned int i = 0; i < vertices_.size(); ++i)
		glVertex3f( vertices_[i]._x, vertices_[i]._y, 0.0);
	glEnd();  
}

void Obstacle::getBoundingBox(int& left, int& right, int& top, int& bottom) const
{
	if (vertices_.size() == 0)
		return;

	left = right = vertices_[0]._x;
	bottom = top = vertices_[0]._y;

	for (unsigned int i = 1; i < vertices_.size(); ++i)
	{
		int x = (int)vertices_[i]._x;
		int y = (int)vertices_[i]._y;
		left = std::min(left, x);
		right = std::max(right, x);
		bottom = std::min(bottom, y);
		top = std::max(top, y);
	}
}

bool Obstacle::clear(const Vector2& v, float radius) const
{
	// 1. check distance from v to obstacle
	for (unsigned int i = 1; i < vertices_.size(); ++i)
	{
		Segment seg(vertices_[i - 1], vertices_[i]);
		float dist = minDistance(seg, v);
		if (dist < radius)
			return false;
	}
	if (isClosed_)
	{
		Segment seg(vertices_[vertices_.size() - 1], vertices_[0]);
		float dist = minDistance(seg, v);
		if (dist < radius)
			return false;
	}

	// 2. check whether v is in the closed obstacle
	if (isClosed_)
	{
		int intersectionCount = 0;
		Vector2 infPoint(v._x, 100000000.f);
		Segment seg2(v, infPoint);
		for (unsigned int i = 1; i < vertices_.size(); ++i)
		{
			Segment seg(vertices_[i - 1], vertices_[i]);
			float dist = minDistance(seg, seg2);
			if (dist < 0.0001f)
				++intersectionCount;
		}
		if (isClosed_)
		{
			Segment seg(vertices_[vertices_.size() - 1], vertices_[0]);
			float dist = minDistance(seg, seg2);
			if (dist < 0.0001f)
				++intersectionCount;
		}
		if (!isBoundingBox_ && intersectionCount % 2 == 1)
			return false;
	}
	return true;
}

bool Obstacle::link(const Vector2& from, const Vector2& to, float radius) const
{
	Segment seg2(from, to);
	for (unsigned int i = 1; i < vertices_.size(); ++i)
	{
		Segment seg(vertices_[i - 1], vertices_[i]);
		float dist = minDistance(seg, seg2);
		if (dist < radius)
			return false;
	}
	if (isClosed_)
	{
		Segment seg(vertices_[vertices_.size() - 1], vertices_[0]);
		float dist = minDistance(seg, seg2);
		if (dist < radius)
			return false;
	}

	return true;
}